#include <algorithm>
#include "los.h"

bool isInLos(const Map& map, const NodeIndex& from, const NodeIndex& to)
{
  int x0 = from.col;
  int y0 = from.row;
  int x1 = to.col;
  int y1 = to.row;

  bool steep = abs(y1 - y0) > abs(x1 - x0);
  if (steep)
  {
    std::swap(x0, y0);
    std::swap(x1, y1);
  }
  if (x0 > x1)
  {
    std::swap(x0, x1);
    std::swap(y0, y1);
  }

  // Delta X and Y
  int dx = x1 - x0;
  int dy = abs(y1 - y0);

  // Error and Delta Error
  float e = 0;
  float de = (float)dy / (float)dx;
  int ystep;
  int y = y0;
  int z;  // Used when calculating line thickness

  // Thickness is based on the ratio dy/dx.  Could be anything
  // from h/2 to h
  float h = 0.99; // height of the cell with a small bit of tolerance
  float thickness = h * (0.5 + (abs(de)*0.5));

  // Since y0 and y1 may have swapped, we need to know which is the
  // min and max
  int minY = std::min<int>(y0, y1);
  int maxY = std::max<int>(y0, y1);

  // Adjust ystep based on whether y0 is below y1
  if (y0 < y1) ystep = 1;
  else         ystep = -1;


  for (int x = x0; x <= x1; ++x)
  {
    if (steep) 
    {
      if (map.isBarrier(x, y)) return false;
      
      // calculate the upper and lower bounds of the region
      // around the line at this step.  Make sure we never
      // check above/below the end points
      z = round(y - (abs(e)+thickness));
      if (z >= minY) if(map.isBarrier(x, z)) return false;

      z = round(y + (abs(e)+thickness));
      if (z <= maxY) if(map.isBarrier(x, z)) return false;
    }
    else       
    {
      if (map.isBarrier(y, x)) return false;

      z = round(y - (abs(e)+thickness));
      if (z >= minY) if (map.isBarrier(z, x)) return false;

      z = round(y + (abs(e)+thickness));
      if (z <= maxY) if (map.isBarrier(z, x)) return false;
    }

    e = e + de;
    if (e >= 0.5)
    {
      y = y + ystep;
      e -= 1.0f;
    }
  }
        
  // If we get here, then a clear path exists between thisnode and node
  return true;
}
